﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Threading;

using HidLibrary;
using System.Runtime.InteropServices;
using Microsoft.Win32.SafeHandles;
using System.Diagnostics;
using System.Threading.Tasks;
namespace ScpControl 
{
    public partial class BusDevice : ScpDevice 
    {

        protected DS4Device[] DS4Controllers = new DS4Device[4];

        //BetterDS4 {
        public DS4Device[] PDS4Controllers {
            get {
                return DS4Controllers;
            }
        }
        //BetterDS4 }

        private class DS4Data
        {
            public byte[] parsedData = new byte[28];
            public byte[] output = new byte[8];
        }
        private DS4Data[] processingData = new DS4Data[4];
        protected UInt32 m_Packet = 0;
        protected ReportEventArgs m_ReportArgs = new ReportEventArgs();
        public event EventHandler<DebugEventArgs> Debug = null;
        public const String BUS_CLASS_GUID = "{F679F562-3164-42CE-A4DB-E7DDBE723909}";
        private Thread[] workers = new Thread[4];
        protected Task workersStatus = null;
        protected bool isWorkersShouldRun = false;

        protected virtual Int32 Scale(Int32 Value, Boolean Flip) 
        {
            Value -= 0x80;

            if (Value == -128) Value = -127;
            if (Flip) Value *= -1;

            return (Int32)((float) Value * 258.00787401574803149606299212599f);
        }


        public BusDevice() : base(BUS_CLASS_GUID) 
        {
            Initialize();
        }

        public BusDevice(IContainer container) : base(BUS_CLASS_GUID) 
        {
            container.Add(this);
            Initialize();
        }

        public void Initialize()
        {
            InitializeComponent();
            for (int i = 0; i < 4; i++)
            {
                int t = i;
                processingData[i] = new DS4Data();
                workers[i] = new Thread(() => { ProcessData(t); });
            }
        }


        private void timerCallback(object state)
        {
            /*for (int i=0; i<DS4Controllers.Length; i++)
            {
                if (DS4Controllers[i] != null)
                {
                    if (!workers[i].IsBusy)
                    {
                        workers[i].RunWorkerAsync();
                    }
                }
              
            }*/
          
        }

        public override Boolean Open(int Instance = 0) 
        {
            return base.Open(Instance);
        }

        public override Boolean Open(String DevicePath) 
        {
            m_Path = DevicePath;
            m_WinUsbHandle = (IntPtr) INVALID_HANDLE_VALUE;

            if (GetDeviceHandle(m_Path))
            {
                m_IsActive = true;
               
            }

            return true;
        }

        public override Boolean Start() 
        {    
            Monitor.Enter(this);
            if (IsActive)
            {
                int i = 0;
                LogDebug("Starting Xinput mapper...");
                LogDebug("Searching for controllers...");
                int[] pid = { 0x05C4 };
                HidDevices.monitor = this;
                isWorkersShouldRun = true;
                try
                {
                    IEnumerable<HidDevice> devices = HidDevices.Enumerate(0x054C, pid);
                    // XXX consider Unplug/Plugin when a device temporarily goes away
                    foreach (HidDevice device in devices)
                    {
                        int index = i; // copied for closure
                        LogDebug("Found Controller: VID:" + device.Attributes.VendorHexId + " PID:" + device.Attributes.ProductHexId + " at " + device.DevicePath);
                        device.OpenDevice();
                        if (device.IsOpen)
                        {
                            device.Inserted += () =>
                            {
                                Monitor.Enter(this);
                                DS4Device d = DS4Controllers[index] = new DS4Device(device, index);
                                Plugin(index + 1);
                                bool announcedNotReady = false, aliveNow = false;
                                while (!device.Active && !d.Gone)
                                {
                                    Task watchdog = new Task(() =>
                                    {
                                        Monitor.Enter(this);
                                        Monitor.Wait(this, 100);
                                        if (!aliveNow)
                                        {
                                            d.Device.CloseDevice();
                                            abortWorker(index);
                                        }
                                        Monitor.Exit(this);
                                    });
                                    watchdog.Start();
                                    if (d.retrieveData(null) != null)
                                    {
                                        Monitor.Enter(this);
                                        aliveNow = true;
                                        LogDebug("Controller " + (index + 1) + " is ready!");
                                        ledColor color = Global.loadPlayerColor1(index);
                                        d.LedColor = color;
                                        d.sendOutputReport(true);
                                    }
                                    else
                                    {
                                        Monitor.Enter(this);
                                        if (!announcedNotReady)
                                        {
                                            LogDebug("Controller " + (index + 1) + " is not yet connected.");
                                            announcedNotReady = true;
                                        }
                                        Monitor.Wait(this, 1000); // retry 
                                    }
                                }
                                Monitor.Exit(this);
                            };
                            device.Removed += () =>
                            {
                                Monitor.Enter(this);
                                DS4Device d = DS4Controllers[index];
                                if (d == null)
                                {
                                    Monitor.Exit(this);
                                    return;
                                }
                                LogDebug("Controller " + (index + 1) + " is no longer connected.");
                                device.MonitorDeviceEvents = false;
                                stop(index + 1, false);
                                device.Active = false;
                                device.MonitorDeviceEvents = true;
                                while (!device.Active && !d.Gone)
                                {
                                    if (d.retrieveData(null) != null)
                                    {
                                        Monitor.Enter(this);
                                        LogDebug("Controller " + (index + 1) + " is now ready to use.");
                                        ledColor color = Global.loadPlayerColor1(index);
                                        d.LedColor = color;
                                        d.sendOutputReport(true);
                                        workers[i] = new Thread(() => { ProcessData(index); });
                                        workers[i].Start();
                                        break;
                                    }
                                    else
                                    {
                                        Monitor.Enter(this);
                                        Monitor.Wait(this, 1000); // retry 
                                    }
                                }
                                Monitor.Exit(this);
                            };
                            device.MonitorDeviceEvents = true;
                        }
                        else
                        {
                            LogDebug("Could not open controller " + (index + 1) + ".");
                        }

                        i++;
                    }

                    if (i == 0 && DS4Controllers[0] == null)
                    {
                        LogDebug("No controllers connected.");
                        IEnumerable<HidDevice> notDevices = HidDevices.Enumerate();
                        foreach (HidDevice device in notDevices)
                        {
                            if (device.openException != null)
                                LogDebug(device.openException.ToString());
                        }
                    }
                    workersStatus = new Task(() => updateWorkers());
                    workersStatus.Start();
                    Monitor.Pulse(this);
                }
                catch (Exception e)
                {
                    LogDebug(e.ToString());
                }
             
            }
            Monitor.Exit(this);
            return true;
        }

        // XXX FIXME TODO stop the DeviceEventMonitors
        public override Boolean Stop()  
        {
            Monitor.Enter(this);
            if (IsActive)
            {
                isWorkersShouldRun = false;
                for (int i = 1; i <= 4; i++)
                {
                    if (DS4Controllers[i - 1] != null)
                        stop(i, true);
                }
                Unplug(0);
            }

            bool res = base.Stop();
            Monitor.Pulse(this);
            Monitor.Exit(this);
            return res;
        }

        private void stop(int i, bool close)
        {
            DS4Device d = DS4Controllers[i - 1];
            if (d == null)
                return;
            d.Device.CloseDevice();
            abortWorker(i - 1);
            LogDebug("Stopping controller " + i + "...");
            if (close)
            {
                Unplug(i);
                d.Gone = true;
                DS4Controllers[i - 1] = null;
            }
            joinWorker(i - 1);
            LogDebug("Controller " + i + " has stopped.");
        }

        public override Boolean Close() 
        {
            try
            {
                Monitor.Enter(this);
                if (IsActive)
                {
                    isWorkersShouldRun = false;
                    for (int i = 1; i <= 4; i++)
                    {
                        if (DS4Controllers[i - 1] != null)
                        {
                            workers[i - 1].Abort();
                            workers[i - 1].Join();
                            Unplug(i);
                            DS4Controllers[i - 1].Device.CloseDevice();
                            DS4Controllers[i - 1] = null;
                        }
                        Unplug(0);
                    }
                }
            }
            catch
            {

            }
            bool res = base.Close();
            Monitor.Pulse(this);
            Monitor.Exit(this);
            return res;
        }

        /** Touchpad button mappings are different than without the finger down on touchpad. */
        public enum ButtonDuringTouch {
            Up, UpRight, Right, DownRight, Down, DownLeft, Left, UpLeft, DPadIdle, // bottom half of [5] are mapped to 0-7 for directions 8 for none
            Square, Cross, Circle, Triangle, // top half of [5]
            L1, R1, L2, R2, // bottom half of [6]
            Share, Options, L3, R3, // top half of [6]
            PS // bottom half of [7]
        }
        private bool absorbButtons = false;
        // Read buttons during touchpad-active mode, where the mappings are totally different.
        // We do this to make virtual mouse/tablet/touchscreen/keyboard/whatever take precedence. 
        public bool AbsorbButtonState(Byte[] Input, ButtonDuringTouch[] buttons, bool touchpadAbsorbsButtons)
        {
            bool pressed = false;
            foreach (ButtonDuringTouch b in buttons)
            {
                switch (b)
                {
                    case ButtonDuringTouch.Up:
                    case ButtonDuringTouch.UpRight:
                    case ButtonDuringTouch.Right:
                    case ButtonDuringTouch.DownRight:
                    case ButtonDuringTouch.Down:
                    case ButtonDuringTouch.DownLeft:
                    case ButtonDuringTouch.Left:
                    case ButtonDuringTouch.UpLeft:
                    case ButtonDuringTouch.DPadIdle:
                        pressed = (Input[5] & 0xf) == (uint)b;
                        break;
                    case ButtonDuringTouch.Square:
                        pressed = (Input[5] & 0x10) != 0;
                        break;
                    case ButtonDuringTouch.Cross:
                        pressed = (Input[5] & 0x20) != 0;
                        break;
                    case ButtonDuringTouch.Circle:
                        pressed = (Input[5] & 0x40) != 0;
                        break;
                    case ButtonDuringTouch.Triangle:
                        pressed = (Input[5] & 0x80) != 0;
                        break;
                    case ButtonDuringTouch.L1:
                        pressed = (Input[6] & 0x01) != 0;
                        break;
                    case ButtonDuringTouch.R1:
                        pressed = (Input[6] & 0x02) != 0;
                        break;
                    /* case ButtonDuringTouch.L2: // Let triggers flow through normally to not make racing-type games awkward.
                        pressed = (Input[6] & 0x04) != 0;
                        break;
                    case ButtonDuringTouch.R2:
                        pressed = (Input[6] & 0x08) != 0;
                        break; */
                    case ButtonDuringTouch.Share:
                        pressed = (Input[6] & 0x10) != 0;
                        break;
                    case ButtonDuringTouch.Options:
                        pressed = (Input[6] & 0x20) != 0;
                        break;
                    case ButtonDuringTouch.L3:
                        pressed = (Input[6] & 0x40) != 0;
                        break;
                    case ButtonDuringTouch.R3:
                        pressed = (Input[6] & 0x80) != 0;
                        break;
                    case ButtonDuringTouch.PS:
                        pressed = (Input[7] & 0x01) != 0;
                        break;
                }
                if (pressed)
                    break;
            }
            if (pressed && touchpadAbsorbsButtons)
                absorbButtons = true;
            return pressed;
        }

        public virtual void Parse(Byte[] Input, Byte[] Output) 
        {
            Byte Serial = (Byte)(Input[0] + 1);

            for (Int32 Index = 0; Index < 28; Index++) Output[Index] = 0x00;

            Output[0] = 0x1C;
            Output[4] = (Byte)(Input[0] + 1);
            Output[9] = 0x14;

            if ((Input[1] & 0x02) != 0) // Pad is active
            {
                if (!absorbButtons) // While the finger is down on the touchpad, and it is not in disabled mode, prohibit button presses from flowing through to normal Xinput.
                {
                    UInt32 Buttons = (UInt32)((Input[10] << 0) | (Input[11] << 8) | (Input[12] << 16) | (Input[13] << 24));

                    // Share
                    if ((Buttons & (0x1 << 0)) > 0) {
                        // Back
                        Output[10] |= (Byte)(1 << 5);
                    }
                    // L3
                    if ((Buttons & (0x1 << 1)) > 0) {
                        // Left Thumb
                        Output[10] |= (Byte)(1 << 6);
                    }
                    // R3
                    if ((Buttons & (0x1 << 2)) > 0) {
                        // Right Thumb
                        Output[10] |= (Byte)(1 << 7); 
                    }
                    // Options
                    if ((Buttons & (0x1 << 3)) > 0) {
                        // Start
                        Output[10] |= (Byte)(1 << 4);
                    }

                    // Up
                    if ((Buttons & (0x1 << 4)) > 0) {
                        // Up
                        Output[10] |= (Byte)(1 << 0);
                    }
                    // Down
                    if ((Buttons & (0x1 << 5)) > 0) {
                        // Down
                        Output[10] |= (Byte)(1 << 3);
                    }
                    // Right
                    if ((Buttons & (0x1 << 6)) > 0) {
                        // Right
                        Output[10] |= (Byte)(1 << 1);
                    }
                    // Left
                    if ((Buttons & (0x1 << 7)) > 0) {
                        // Left
                        Output[10] |= (Byte)(1 << 2);
                    }

                    // L1
                    if ((Buttons & (0x1 << 10)) > 0) {
                        // Left  Shoulder
                        Output[11] |= (Byte)(1 << 0); 
                    }
                    // R1
                    if ((Buttons & (0x1 << 11)) > 0) {
                        // Right Shoulder
                        Output[11] |= (Byte)(1 << 1); 
                    }

                    // Triangle
                    if ((Buttons & (0x1 << 12)) > 0) {
                        // Y
                        Output[11] |= (Byte)(1 << 7); 
                    }
                    // Circle
                    if ((Buttons & (0x1 << 13)) > 0) {
                        // B
                        Output[11] |= (Byte)(1 << 5);
                    }
                    // X
                    if ((Buttons & (0x1 << 14)) > 0) {
                        // A
                        Output[11] |= (Byte)(1 << 4);
                    }
                    //Square
                    if ((Buttons & (0x1 << 15)) > 0) {
                        // X
                        Output[11] |= (Byte)(1 << 6);
                    }

                    //PS
                    if ((Buttons & (0x1 << 16)) > 16) {
                        // Guide
                        Output[11] |= (Byte)(1 << 2);
                    }
                }
                
                //L2
                Output[12] = Input[26]; // Left Trigger
                //R2
                Output[13] = Input[27]; // Right Trigger
            
                Int32 ThumbLX =  Scale(Input[14], false);
                Int32 ThumbLY = -Scale(Input[15], false);
                Int32 ThumbRX = Scale(Input[16], false);
                Int32 ThumbRY = -Scale(Input[17], false);

                // Left Stick (X)
                Output[14] = (Byte)((ThumbLX >> 0) & 0xFF); // LX
                Output[15] = (Byte)((ThumbLX >> 8) & 0xFF);

                // Left Stick (Y)
                Output[16] = (Byte)((ThumbLY >> 0) & 0xFF); // LY
                Output[17] = (Byte)((ThumbLY >> 8) & 0xFF);

                // Right Stick (X)
                Output[18] = (Byte)((ThumbRX >> 0) & 0xFF); // RX
                Output[19] = (Byte)((ThumbRX >> 8) & 0xFF);

                // Right Stick (Y)
                Output[20] = (Byte)((ThumbRY >> 0) & 0xFF); // RY
                Output[21] = (Byte)((ThumbRY >> 8) & 0xFF);
            }
        }


        public virtual Boolean Plugin(Int32 Serial) 
        {
            if (IsActive)
            {
                Int32 Transfered = 0;
                Byte[] Buffer = new Byte[16];

                Buffer[0] = 0x10;
                Buffer[1] = 0x00;
                Buffer[2] = 0x00;
                Buffer[3] = 0x00;

                Buffer[4] = (Byte)((Serial >>  0) & 0xFF);
                Buffer[5] = (Byte)((Serial >>  8) & 0xFF);
                Buffer[6] = (Byte)((Serial >> 16) & 0xFF);
                Buffer[7] = (Byte)((Serial >> 24) & 0xFF);

                return DeviceIoControl(m_FileHandle, 0x2A4000, Buffer, Buffer.Length, null, 0, ref Transfered, IntPtr.Zero);
            }

            return false;
        }

        public virtual Boolean Unplug(Int32 Serial) 
        {
            if (IsActive)
            {
                Int32 Transfered = 0;
                Byte[] Buffer = new Byte[16];

                Buffer[0] = 0x10;
                Buffer[1] = 0x00;
                Buffer[2] = 0x00;
                Buffer[3] = 0x00;

                Buffer[4] = (Byte)((Serial >>  0) & 0xFF);
                Buffer[5] = (Byte)((Serial >>  8) & 0xFF);
                Buffer[6] = (Byte)((Serial >> 16) & 0xFF);
                Buffer[7] = (Byte)((Serial >> 24) & 0xFF);

                return DeviceIoControl(m_FileHandle, 0x2A4004, Buffer, Buffer.Length, null, 0, ref Transfered, IntPtr.Zero);
            }

            return false;
        }

        public void updateWorkers()
        {
            Monitor.Enter(this);
            while (isWorkersShouldRun)
            {
                for (int i = 0; i < DS4Controllers.Length; i++)
                {                    
                    if (DS4Controllers[i] != null)
                    {
                        int t = i;
                        if (workers[i].ThreadState != System.Threading.ThreadState.Unstarted)
                            workers[i] = new Thread(() => { ProcessData(t); });
                        workers[i].Start();
                    }
                }
                Monitor.Wait(this);
            }
            for (int i = 0; i < DS4Controllers.Length; i++)
            {
                if (DS4Controllers[i] != null)
                    abortWorker(i);
            }
            for (int i = 0; i < DS4Controllers.Length; i++)
            {
                if (DS4Controllers[i] != null)
                    joinWorker(i);
            }
            Monitor.Exit(this);
            LogDebug("Stopped Xinput mapper!");
        }

        internal void abortWorker(int i)
        {
            workers[i].Abort();
        }
        internal void joinWorker(int i)
        {
            try
            {
                Monitor.Exit(this);
                workers[i].Join();
                Monitor.Enter(this);
            }
            catch { }
        }

        public void ProcessData(int device)
        {
            Monitor.Enter(this);
            try
            {
                for (; ;) // drop the monitor inside the work loop so other things can get a chance
                {
                    DS4Device d = DS4Controllers[device];
                    if (d == null)
                    {
                        Monitor.Exit(this);
                        return;
                    }
                    absorbButtons = false;
                    byte[] data = DS4Controllers[device].retrieveData(this);
                    if (data == null)
                        return;
                    Monitor.Enter(this);
                    data[0] = (byte)device;
                    Parse(data, processingData[device].parsedData);
                    Report(processingData[device].parsedData, processingData[device].output);
                }
            }
            catch (Exception)
            {
                // killed
            }
        }

        public virtual Boolean LogDebug(byte[] array)
        {
            string dataStr = "";
            for (uint i = 0; i < array.Length; i++)
            {
                dataStr = String.Format("{0}{1:X2} ", dataStr, array[i]);
            }
            DebugEventArgs args = new DebugEventArgs(dataStr);

            On_Debug(this, args);
            return true;
        }

        // do explicit locking here later if necessary
        private bool hapticFeedbackInProgress = false;
        /** Attempt to perform haptic feedback.  The caller will run this asynchronously and within it generate whatever pattern we want. */
        public bool PerformHapticFeedback(Action withFeedbackEnabled)
        {
            if (hapticFeedbackInProgress)
                return false;
            hapticFeedbackInProgress = true;
            HapticBigRumble = HapticSmallRumble = 0;
            new Task(() =>
            {
                withFeedbackEnabled();
                hapticFeedbackInProgress = false;
            }).Start();
            return true;
        }
        private Byte hapticBigRumble, hapticSmallRumble;
        public Byte HapticBigRumble
        {
            set { hapticBigRumble = value; }
        }
        public Byte HapticSmallRumble
        {
            set { hapticSmallRumble = value; }
        }
        private void assignHapticFeedbackRumbles(DS4Device d)
        {
            d.BigRumble = hapticBigRumble;
            d.SmallRumble = hapticSmallRumble;
        }

        public virtual Boolean Report(Byte[] Input, Byte[] Output) 
        {
            if (IsActive)
            {
                Int32 Transfered = 0;

                bool result = DeviceIoControl(m_FileHandle, 0x2A400C, Input, Input.Length, Output, Output.Length, ref Transfered, IntPtr.Zero) && Transfered > 0;
                DS4Device d = DS4Controllers[Input[4] - 1];
                if (hapticFeedbackInProgress)
                {
                    assignHapticFeedbackRumbles(d);
                }
                else if (result == true && Output[1] == 0x08)
                {
                    d.BigRumble = (Byte)Output[3];
                    d.SmallRumble = (Byte)Output[4];
                }
                d.sendOutputReport();
                return result;

            }
            return false;
        }

        public virtual Boolean LogDebug(String Data)
        {
            DebugEventArgs args = new DebugEventArgs(Data);

            On_Debug(this, args);

            return true;
        }

        protected virtual void On_Debug(object sender, DebugEventArgs e)
        {
            Console.WriteLine(e.Time.ToString() + " " + e.Data);
            if (Debug != null) Debug(sender, e);
        }

        public void sendUpdateReport(int device)
        {
            DS4Controllers[device].sendOutputReport();
        }

        public void SetLeds(int deviceNum,byte Red, byte Green, byte Blue)
        {
            DS4Controllers[deviceNum].setLedColor(Red, Green, Blue);
        }

        public void setRumble(int deviceNum, byte Boost, byte Left, byte Right)
        {
            DS4Controllers[deviceNum].BigRumble = Left;
            DS4Controllers[deviceNum].SmallRumble = Right;
            DS4Controllers[deviceNum].RumbleBoost = Boost;
        }

        public string getControllerInfo(int device)
        {
            if (DS4Controllers[device] != null)
                return DS4Controllers[device].toString();
            else
                return null;
          
        }

    }

}